#include <gtest/gtest.h>
#include <gmock/gmock.h>

#define private public
#define protected public
#include <element.h>
#undef private
#undef protected
#include <iostream>

using namespace std;
using ::testing::ContainerEq;

// helper functions
inline double euclideanDistance(const StdArray6d &pi, const StdArray6d &pj)
{
    double ans = 0.0;
    for (int i = 0; i < 3; i++)
    {
        ans += (pi[i] - pj[i]) * (pi[i] - pj[i]);
    }
    return sqrt(ans);
};

inline double euclideanDistance(const StdArray6d* pi, const StdArray6d* pj)
{
    double ans = 0.0;
    for (int i = 0; i < 3; i++)
    {
        ans += ((*pi)[i] - (*pj)[i]) * ((*pi)[i] - (*pj)[i]);
    }
    return sqrt(ans);
};

inline void printStdArray6d(const StdArray6d &arr)
{
    for (int i = 0; i < 6; i++)
    {
        cout << arr[i] << ", ";
    }
    cout << endl;
}

inline void printStdArray6d(const StdArray6d* arr)
{
    for (int i = 0; i < 6; i++)
    {
        cout << (*arr)[i] << ", ";
    }
    cout << endl;
}

class Link3DLDTest: public::testing::Test
{
    protected:
        // Declares the Link3DLDs
        // Declares particles
        Particle* p1;
        Particle* p2;

        Particle* p3;
        Particle* p4;

        Particle* p5;
        Particle* p6;

        Particle* p7;
        Particle* p8;
        Particle* p9;

        // Declares materials
        double E , Et, density;
        LinearElastic* mat1;
        UniBilinear* mat2;

        // Declares sections
        Rectangle* sect1;
        AngleSteel* sect2;

        // Declares elements
        Link3DLD* e1;
        Link3DLD* e2;
        Link3DLD* e3;

        Link3DLD* e4;
        Link3DLD* e5;

        // other data

        // You can remove any or all of the following functions if their bodies
        // would be empty.
        Link3DLDTest()
        {
            // You can do set-up work for each test here.
            // prepare particles
            p1 = new Particle(1, 0, 0, 0);
            p2 = new Particle(2, 10, 0, 0);

            p3 = new Particle(3, 0, 0, 0);
            p4 = new Particle(4, 0, 0, 10);

            p5 = new Particle(5, 0, 0, 0);
            p6 = new Particle(6, 10, 0, 0);

            p7 = new Particle(7, 0, 0, 0);
            p8 = new Particle(8, 10, 0, 0);
            p9 = new Particle(9, 10, 10, 0);

            // prepare materials
            E = 1e3, Et=1e2, density = 10;
            mat1 = new LinearElastic(1);
            mat1->setE(E);
            mat1->setDensity(density);

            mat2 = new UniBilinear(2, E, Et, 20, 0);
            mat2->setDensity(density);

            // prepare sections
            sect1 = new Rectangle(1, 20, 40);
            sect2 = new AngleSteel(2, 10, 10, 2, 2);

            // prepare elements
            e1 = new Link3DLD(1, p1, p2, mat1, sect1);
            e2 = new Link3DLD(2, p3, p4, mat2, sect1);
            e3 = new Link3DLD(3, p5, p6, mat1, sect2);

            e4 = new Link3DLD(4, p7, p8, mat1, sect1);
            e5 = new Link3DLD(5, p7, p9, mat1, sect1);
        };

        ~Link3DLDTest()
        {
            // You can do clean-up work that doesn't throw exceptions here.
            // delete particles
            delete p1, p2, p3, p4, p5, p6, p7, p8, p9;

            // delete materials
            delete mat1, mat2;

            // delete sections
            delete sect1, sect2;

            // delete elements
            delete e1, e2, e3, e4, e5;
        };

        // If the constructor and destructor are not enough for setting up
        // and cleaning up each test, you can define the following methods:
        void SetUp() override
        {
            // Code here will be called immediately after the constructor
            // (right before each test).
            cout << "Set Up Link3DLD Test" << endl;
        };

        void TearDown() override
        {
            // Code here will be called immediately after each test (right
            // before the destructor).
        };

        // Class members declared here can be used by all tests in the test
        // suite for Foo.
};



// Tests that the Link3DLD::Link3DLD() method does Abc.
TEST_F(Link3DLDTest, IsEmptyInitiate)
{
    cout << "\n---------- Link3DLDTest IsEmptyInitiate ----------" << endl;
    // initiated by BaseElement
    cout << "check id... " << endl;
    EXPECT_EQ(e1->id(), 1);
    EXPECT_EQ(e2->id(), 2);
    EXPECT_EQ(e3->id(), 3);
    EXPECT_EQ(e4->id(), 4);
    EXPECT_EQ(e5->id(), 5);

    cout << "check type... " << endl;
    char type[] = "Link3DLD";
    EXPECT_STREQ(type, e1->type_.c_str());

    cout << "check num_of_particles... " << endl;
    EXPECT_EQ(2, e1->num_of_particles_);

    cout << "check particles container... " << endl;
    auto iter = e1->particles_.begin();
    EXPECT_EQ(p1, iter->second);
    iter++;
    EXPECT_EQ(p2, iter->second);

    iter = e2->particles_.begin();
    EXPECT_EQ(p3, iter->second);
    iter++;
    EXPECT_EQ(p4, iter->second);

    iter = e3->particles_.begin();
    EXPECT_EQ(p5, iter->second);
    iter++;
    EXPECT_EQ(p6, iter->second);

    iter = e4->particles_.begin();
    EXPECT_EQ(p7, iter->second);
    iter++;
    EXPECT_EQ(p8, iter->second);

    iter = e5->particles_.begin();
    EXPECT_EQ(p7, iter->second);
    iter++;
    EXPECT_EQ(p9, iter->second);

    cout << "check material... " << endl;
    EXPECT_EQ(mat1, e1->material_);
    EXPECT_EQ(mat2, e2->material_);
    EXPECT_EQ(mat1, e3->material_);
    EXPECT_EQ(mat1, e4->material_);
    EXPECT_EQ(mat1, e5->material_);

    cout << "check section... " << endl;
    EXPECT_EQ(sect1, e1->section_);
    EXPECT_EQ(sect1, e2->section_);
    EXPECT_EQ(sect2, e3->section_);
    EXPECT_EQ(sect1, e4->section_);
    EXPECT_EQ(sect1, e5->section_);

    cout << "check mass... " << endl;
    double mass1 = density * e1->length_ * e1->section_->A();
    double mass2 = density * e2->length_ * e2->section_->A();
    double mass3 = density * e3->length_ * e3->section_->A();
    double mass4 = density * e4->length_ * e4->section_->A();
    double mass5 = density * e5->length_ * e5->section_->A();
    EXPECT_DOUBLE_EQ(mass1, e1->mass_);
    EXPECT_DOUBLE_EQ(mass2, e2->mass_);
    EXPECT_DOUBLE_EQ(mass3, e3->mass_);
    EXPECT_DOUBLE_EQ(mass4, e4->mass_);
    EXPECT_DOUBLE_EQ(mass5, e5->mass_);

    cout << "check strain... " << endl;
    EXPECT_DOUBLE_EQ(0.0, e1->strain_.sx);
    EXPECT_DOUBLE_EQ(0.0, e1->strain_.sy);
    EXPECT_DOUBLE_EQ(0.0, e1->strain_.sz);
    EXPECT_DOUBLE_EQ(0.0, e1->strain_.sxy);
    EXPECT_DOUBLE_EQ(0.0, e1->strain_.syz);
    EXPECT_DOUBLE_EQ(0.0, e1->strain_.sxz);

    cout << "check stress... " << endl;
    EXPECT_DOUBLE_EQ(0.0, e1->stress_.sx);
    EXPECT_DOUBLE_EQ(0.0, e1->stress_.sy);
    EXPECT_DOUBLE_EQ(0.0, e1->stress_.sz);
    EXPECT_DOUBLE_EQ(0.0, e1->stress_.sxy);
    EXPECT_DOUBLE_EQ(0.0, e1->stress_.syz);
    EXPECT_DOUBLE_EQ(0.0, e1->stress_.sxz);

    cout << "check ex, ey, ez... " << endl;
    // e1
    EXPECT_DOUBLE_EQ(1.0, e1->ex_(0));
    EXPECT_DOUBLE_EQ(0.0, e1->ex_(1));
    EXPECT_DOUBLE_EQ(0.0, e1->ex_(2));
    for (int i = 0; i < 3; i++)
    {
        EXPECT_DOUBLE_EQ(0.0, e1->ey_(i));
        EXPECT_DOUBLE_EQ(0.0, e1->ez_(i));
    }

    // e4
    EXPECT_DOUBLE_EQ(1.0 / sqrt(2), e5->ex_(0));
    EXPECT_DOUBLE_EQ(1.0 / sqrt(2), e5->ex_(1));
    EXPECT_DOUBLE_EQ(0.0, e5->ex_(2));
    for (int i = 0; i < 3; i++)
    {
        EXPECT_DOUBLE_EQ(0.0, e5->ey_(i));
        EXPECT_DOUBLE_EQ(0.0, e5->ez_(i));
    }

    cout << "check is_failed... " << endl;
    EXPECT_FALSE(e1->is_failed_);
    EXPECT_FALSE(e2->is_failed_);
    EXPECT_FALSE(e3->is_failed_);
    EXPECT_FALSE(e4->is_failed_);
    EXPECT_FALSE(e5->is_failed_);

    // initiated by StructElement
    cout << "check pi, pj... " << endl;
    EXPECT_EQ(p1, e1->pi_);
    EXPECT_EQ(p2, e1->pj_);
    EXPECT_EQ(p3, e2->pi_);
    EXPECT_EQ(p4, e2->pj_);
    EXPECT_EQ(p5, e3->pi_);
    EXPECT_EQ(p6, e3->pj_);
    EXPECT_EQ(p7, e4->pi_);
    EXPECT_EQ(p8, e4->pj_);
    EXPECT_EQ(p7, e5->pi_);
    EXPECT_EQ(p9, e5->pj_);

    cout << "check fi, fj... " << endl;
    StdArray6d target = {0, 0, 0, 0, 0, 0};
    EXPECT_THAT(target, ContainerEq(e1->fi_));
    EXPECT_THAT(target, ContainerEq(e2->fi_));
    EXPECT_THAT(target, ContainerEq(e3->fi_));
    EXPECT_THAT(target, ContainerEq(e4->fi_));
    EXPECT_THAT(target, ContainerEq(e1->fj_));
    EXPECT_THAT(target, ContainerEq(e2->fj_));
    EXPECT_THAT(target, ContainerEq(e3->fj_));
    EXPECT_THAT(target, ContainerEq(e4->fj_));

    cout << "check length... " << endl;
    EXPECT_DOUBLE_EQ(10.0, e1->length_);
    EXPECT_DOUBLE_EQ(10.0, e2->length_);
    EXPECT_DOUBLE_EQ(10.0, e3->length_);
    EXPECT_DOUBLE_EQ(10.0, e4->length_);
    EXPECT_DOUBLE_EQ(10.0 * sqrt(2), e5->length_);

    cout << "check lt_, la_... " << endl;
    EXPECT_DOUBLE_EQ(10.0, e1->lt_);
    EXPECT_DOUBLE_EQ(10.0, e2->lt_);
    EXPECT_DOUBLE_EQ(10.0, e3->lt_);
    EXPECT_DOUBLE_EQ(10.0, e4->lt_);
    EXPECT_DOUBLE_EQ(10.0 * sqrt(2), e5->lt_);
    EXPECT_DOUBLE_EQ(10.0, e1->la_);
    EXPECT_DOUBLE_EQ(10.0, e2->la_);
    EXPECT_DOUBLE_EQ(10.0, e3->la_);
    EXPECT_DOUBLE_EQ(10.0, e4->la_);
    EXPECT_DOUBLE_EQ(10.0 * sqrt(2), e5->la_);

    cout << "check element_force... " << endl;
    EXPECT_DOUBLE_EQ(0.0, e1->element_force_.fx);
    EXPECT_DOUBLE_EQ(0.0, e1->element_force_.sfy);
    EXPECT_DOUBLE_EQ(0.0, e1->element_force_.sfy);
    EXPECT_DOUBLE_EQ(0.0, e1->element_force_.mx);
    EXPECT_DOUBLE_EQ(0.0, e1->element_force_.my);
    EXPECT_DOUBLE_EQ(0.0, e1->element_force_.mz);

    // initiate by Link3DLD
    cout << "check posi, posj... " << endl;
    EXPECT_EQ(p1->current_position(), e1->posi_);
    EXPECT_EQ(p2->current_position(), e1->posj_);
    EXPECT_EQ(p3->current_position(), e2->posi_);
    EXPECT_EQ(p4->current_position(), e2->posj_);
    EXPECT_EQ(p5->current_position(), e3->posi_);
    EXPECT_EQ(p6->current_position(), e3->posj_);
    EXPECT_EQ(p7->current_position(), e4->posi_);
    EXPECT_EQ(p8->current_position(), e4->posj_);
    EXPECT_EQ(p7->current_position(), e5->posi_);
    EXPECT_EQ(p9->current_position(), e5->posj_);

    cout << "check particle dof..." << endl;
    std::set<std::string> key;
    key.insert("Ux");
    key.insert("Uy");
    key.insert("Uz");
    EXPECT_THAT(key, ContainerEq(p1->dof_key_));
    EXPECT_THAT(key, ContainerEq(p2->dof_key_));
    EXPECT_THAT(key, ContainerEq(p3->dof_key_));

    cout << "check particle mass..." << endl;
    double m1 = mass1 / 2.0;
    double m2 = mass1 / 2.0;
    double m3 = mass2 / 2.0;
    double m4 = mass2 / 2.0;
    double m5 = mass3 / 2.0;
    double m6 = mass3 / 2.0;
    double m7 = mass4 / 2.0 + mass5 / 2.0;
    double m8 = mass4 / 2.0;
    double m9 = mass5 / 2.0;

    StdArray6d pm1 = {m1, m1, m1, 0, 0, 0};
    StdArray6d pm2 = {m2, m2, m2, 0, 0, 0};
    StdArray6d pm3 = {m3, m3, m3, 0, 0, 0};
    StdArray6d pm4 = {m4, m4, m4, 0, 0, 0};
    StdArray6d pm5 = {m5, m5, m5, 0, 0, 0};
    StdArray6d pm6 = {m6, m6, m6, 0, 0, 0};
    StdArray6d pm7 = {m7, m7, m7, 0, 0, 0};
    StdArray6d pm8 = {m8, m8, m8, 0, 0, 0};
    StdArray6d pm9 = {m9, m9, m9, 0, 0, 0};

    EXPECT_THAT(pm1, ContainerEq(p1->mass()->lumped_mass));
    EXPECT_THAT(pm2, ContainerEq(p2->mass()->lumped_mass));
    EXPECT_THAT(pm3, ContainerEq(p3->mass()->lumped_mass));
    EXPECT_THAT(pm4, ContainerEq(p4->mass()->lumped_mass));
    EXPECT_THAT(pm5, ContainerEq(p5->mass()->lumped_mass));
    EXPECT_THAT(pm6, ContainerEq(p6->mass()->lumped_mass));
    EXPECT_THAT(pm7, ContainerEq(p7->mass()->lumped_mass));
    EXPECT_THAT(pm8, ContainerEq(p8->mass()->lumped_mass));
    EXPECT_THAT(pm9, ContainerEq(p9->mass()->lumped_mass));

    Eigen::Matrix3d m, Im;
    m.setZero();
    Im.setZero();
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(m(i,j), p1->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p2->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p3->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p4->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p5->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p6->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p7->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p8->mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p9->mass()->translate_mass_matrix(i,j));

            // rotation mass matrix
            EXPECT_DOUBLE_EQ(Im(i,j), p1->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p2->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p3->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p4->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p5->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p6->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p7->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p8->mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p9->mass()->rotation_mass_matrix(i,j));
        }
    }

    cout << "check translate_mass_matrix_on... " << endl;
    EXPECT_FALSE(p1->translate_mass_matrix_on_);
    EXPECT_FALSE(p2->translate_mass_matrix_on_);
    EXPECT_FALSE(p3->translate_mass_matrix_on_);
    EXPECT_FALSE(p4->translate_mass_matrix_on_);
    EXPECT_FALSE(p5->translate_mass_matrix_on_);
    EXPECT_FALSE(p6->translate_mass_matrix_on_);
    EXPECT_FALSE(p7->translate_mass_matrix_on_);
    EXPECT_FALSE(p8->translate_mass_matrix_on_);
    EXPECT_FALSE(p9->translate_mass_matrix_on_);

    cout << "check rotation_mass_matrix_on... " << endl;
    EXPECT_FALSE(p1->rotation_mass_matrix_on_);
    EXPECT_FALSE(p2->rotation_mass_matrix_on_);
    EXPECT_FALSE(p3->rotation_mass_matrix_on_);
    EXPECT_FALSE(p4->rotation_mass_matrix_on_);
    EXPECT_FALSE(p5->rotation_mass_matrix_on_);
    EXPECT_FALSE(p6->rotation_mass_matrix_on_);
    EXPECT_FALSE(p7->rotation_mass_matrix_on_);
    EXPECT_FALSE(p8->rotation_mass_matrix_on_);
    EXPECT_FALSE(p9->rotation_mass_matrix_on_);
};


// Tests that the Link3DLD::calcElementForce() method.
TEST_F(Link3DLDTest, CalcElementForce)
{
    cout << "\n---------- Link3DLDTest calcElementForce ----------" << endl;
    cout << "set a diplace to check Link3DLD internal force..." << endl;
    StdArray6d dx = {0, 0, 0, 0, 0, 0};
    StdArray6d c = {0, 0, 0, 0, 0, 0};
    double la = 0, lt = 0, strain = 0, stress = 0, fx = 0;
    StdArray6d fi = {0, 0, 0, 0, 0, 0};
    StdArray6d fj = {0, 0, 0, 0, 0, 0};

    // check e1 again, dx = {1, 2, 3, 4, 5};
    dx = {1, -2, 3, 4, 5};
    p2->setDisplace(dx);
    p2->updatePosition();
    c = p2->coordinate_;
    for (int i = 0; i < 6; i++)
    {
        c[i] += dx[i];
    }
    EXPECT_THAT(c, ContainerEq(p2->current_position_));

    // targets
    lt = euclideanDistance(p1->current_position_, p2->current_position_);
    la = e1->length_;
    strain = (lt - la) / e1->length_;
    stress = strain * e1->material_->E();
    fx = e1->section_->A() * stress;
    e1->calcElementForce();

    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt - e1->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain - e1->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress - e1->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx - e1->element_force_.fx) < 1e-10);

    // check fi_, fj_
    fi.fill(0), fj.fill(0);
    fi[0] = fx * e1->ex_(0);
    fi[1] = fx * e1->ex_(1);
    fi[2] = fx * e1->ex_(2);
    fj[0] = -fx * e1->ex_(0);
    fj[1] = -fx * e1->ex_(1);
    fj[2] = -fx * e1->ex_(2);
    EXPECT_THAT(fi, ContainerEq(e1->fi_));
    EXPECT_THAT(fj, ContainerEq(e1->fj_));


    // check e2
    int i = 0;
    dx.fill(0);
    dx[i] = 0.01 * (i + 1);
    p4->setDisplace(dx);
    p4->updatePosition();
    c = p4->coordinate_;
    c[i] += dx[i];
    EXPECT_THAT(c, ContainerEq(p4->current_position_));
    // targets
    lt = euclideanDistance(p3->current_position_, p4->current_position_);
    la = e2->length_;
    strain = (lt - la) / e2->length_;
    stress = strain * e2->material_->E();
    fx = e2->section_->A() * stress;
    e2->calcElementForce();
    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt - e2->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain - e2->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress - e2->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx - e2->element_force_.fx) < 1e-10);
    // check fi_, fj_
    fi.fill(0), fj.fill(0);
    fi[0] = fx * e2->ex_(0);
    fi[1] = fx * e2->ex_(1);
    fi[2] = fx * e2->ex_(2);
    fj[0] = -fx * e2->ex_(0);
    fj[1] = -fx * e2->ex_(1);
    fj[2] = -fx * e2->ex_(2);
    EXPECT_THAT(fi, ContainerEq(e2->fi_));
    EXPECT_THAT(fj, ContainerEq(e2->fj_));


    // check e3
    i = 1;
    dx.fill(0);
    dx[i] = 0.01 * (i + 1);
    p5->setDisplace(dx);
    p5->updatePosition();
    c = p5->coordinate_;
    c[i] += dx[i];
    EXPECT_THAT(c, ContainerEq(p5->current_position_));
    // targets
    lt = euclideanDistance(p5->current_position_, p6->current_position_);
    la = e3->length_;
    strain = (lt - la) / e3->length_;
    stress = strain * e3->material_->E();
    fx = e3->section_->A() * stress;
    e3->calcElementForce();
    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt - e3->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain - e3->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress - e3->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx - e3->element_force_.fx) < 1e-10);
    // check fi_, fj_
    fi.fill(0), fj.fill(0);
    fi[0] = fx * e3->ex_(0);
    fi[1] = fx * e3->ex_(1);
    fi[2] = fx * e3->ex_(2);
    fj[0] = -fx * e3->ex_(0);
    fj[1] = -fx * e3->ex_(1);
    fj[2] = -fx * e3->ex_(2);
    EXPECT_THAT(fi, ContainerEq(e3->fi_));
    EXPECT_THAT(fj, ContainerEq(e3->fj_));

    // check e4 and e5
    double lt4 = 0, lt5 = 0, la4 = e4->la_, la5 = e5->la_;
    double strain4 = 0, strain5 = 0, stress4 = 0, stress5 = 0;
    double fx4 = 0, fx5 = 0;
    StdArray6d fi4, fj4, fi5, fj5;
    fi4.fill(0), fj4.fill(0), fj5.fill(0), fj5.fill(0);

    i = 0;
    dx.fill(0);
    dx[i] = 0.01 * (i + 1);
    p7->setDisplace(dx);
    p7->updatePosition();
    c = p7->coordinate_;
    c[i] += dx[i];
    EXPECT_THAT(c, ContainerEq(p7->current_position_));
    // targets
    lt4 = euclideanDistance(p7->current_position_, p8->current_position_);
    la4 = e4->length_;
    strain4 = (lt4 - la4) / e4->length_;
    stress4 = strain4 * e4->material_->E();
    fx4 = e4->section_->A() * stress4;
    e4->calcElementForce();
    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt4 - e4->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain4 - e4->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress4 - e4->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx4 - e4->element_force_.fx) < 1e-10);
    lt5 = euclideanDistance(p7->current_position_, p9->current_position_);
    la5 = e5->length_;
    strain5 = (lt5 - la5) / e5->length_;
    stress5 = strain5 * e5->material_->E();
    fx5 = e5->section_->A() * stress5;
    e5->calcElementForce();
    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt5 - e5->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain5 - e5->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress5 - e5->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx5 - e5->element_force_.fx) < 1e-10);
    // check fi_, fj_
    fi4.fill(0), fj4.fill(0);
    fi4[0] = fx4 * e4->ex_(0);
    fi4[1] = fx4 * e4->ex_(1);
    fi4[2] = fx4 * e4->ex_(2);
    fj4[0] = -fx4 * e4->ex_(0);
    fj4[1] = -fx4 * e4->ex_(1);
    fj4[2] = -fx4 * e4->ex_(2);
    EXPECT_THAT(fi4, ContainerEq(e4->fi_));
    EXPECT_THAT(fj4, ContainerEq(e4->fj_));
    // check fi_, fj_
    fi5.fill(0), fj5.fill(0);
    fi5[0] = fx5 * e5->ex_(0);
    fi5[1] = fx5 * e5->ex_(1);
    fi5[2] = fx5 * e5->ex_(2);
    fj5[0] = -fx5 * e5->ex_(0);
    fj5[1] = -fx5 * e5->ex_(1);
    fj5[2] = -fx5 * e5->ex_(2);
    EXPECT_THAT(fi5, ContainerEq(e5->fi_));
    EXPECT_THAT(fj5, ContainerEq(e5->fj_));
};


// Tests that the Link3DLD::setParticleForce() method.
TEST_F(Link3DLDTest, SetParticleForce)
{
    cout << "\n---------- Link3DLDTest setParticleForce ----------" << endl;
    cout << "please check calcElementForce() fist!" << endl;
    StdArray6d dx = {0, 0, 0, 0, 0, 0};
    StdArray6d c = {0, 0, 0, 0, 0, 0};
    double la = 0, lt = 0, strain = 0, stress = 0, fx = 0;
    StdArray6d fi = {0, 0, 0, 0, 0, 0};
    StdArray6d fj = {0, 0, 0, 0, 0, 0};

    // check e1
    int i = 0;
    dx.fill(0);
    dx[i] = 0.01 * (i + 1);
    p1->clearForce();
    p2->clearForce();
    p2->setDisplace(dx);
    p2->updatePosition();
    e1->setParticleForce();
    // targets
    lt = euclideanDistance(p1->current_position_, p2->current_position_);
    la = e1->length_;
    strain = (lt - la) / e1->length_;
    stress = strain * e1->material_->E();
    fx = e1->section_->A() * stress;
    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt - e1->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain - e1->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress - e1->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx - e1->element_force_.fx) < 1e-10);
    // check fi_, fj_
    fi.fill(0), fj.fill(0);
    fi[0] = fx * e1->ex_(0);
    fi[1] = fx * e1->ex_(1);
    fj[0] = -fx * e1->ex_(0);
    fj[1] = -fx * e1->ex_(1);
    EXPECT_THAT(fi, ContainerEq(e1->fi_));
    EXPECT_THAT(fj, ContainerEq(e1->fj_));
    // check particle force
    EXPECT_THAT(fi, ContainerEq(p1->force_));
    EXPECT_THAT(fj, ContainerEq(p2->force_));

    // check e4 and e5
    double lt4 = 0, lt5 = 0, la4 = e4->la_, la5 = e5->la_;
    double strain4 = 0, strain5 = 0, stress4 = 0, stress5 = 0;
    double fx4 = 0, fx5 = 0;
    StdArray6d fi4, fj4, fi5, fj5;
    fi4.fill(0), fj4.fill(0), fj5.fill(0), fj5.fill(0);

    i = 0;
    p7->clearForce();
    p8->clearForce();
    p9->clearForce();
    dx.fill(0);
    dx[i] = 0.01 * (i + 1);
    p7->setDisplace(dx);
    p7->updatePosition();
    e4->setParticleForce();
    e5->setParticleForce();
    // targets
    lt4 = euclideanDistance(p7->current_position_, p8->current_position_);
    la4 = e4->length_;
    strain4 = (lt4 - la4) / e4->length_;
    stress4 = strain4 * e4->material_->E();
    fx4 = e4->section_->A() * stress4;
    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt4 - e4->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain4 - e4->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress4 - e4->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx4 - e4->element_force_.fx) < 1e-10);
    lt5 = euclideanDistance(p7->current_position_, p9->current_position_);
    la5 = e5->length_;
    strain5 = (lt5 - la5) / e5->length_;
    stress5 = strain5 * e5->material_->E();
    fx5 = e5->section_->A() * stress5;
    // lt, strain, stress, elment_force
    EXPECT_TRUE(fabs(lt5 - e5->lt_) < 1e-10);
    EXPECT_TRUE(fabs(strain5 - e5->strain_.sx) < 1e-10);
    EXPECT_TRUE(fabs(stress5 - e5->stress_.sx) < 1e-10);
    EXPECT_TRUE(fabs(fx5 - e5->element_force_.fx) < 1e-10);
    // check fi_, fj_
    fi4.fill(0), fj4.fill(0);
    fi4[0] = fx4 * e4->ex_(0);
    fi4[1] = fx4 * e4->ex_(1);
    fj4[0] = -fx4 * e4->ex_(0);
    fj4[1] = -fx4 * e4->ex_(1);
    EXPECT_THAT(fi4, ContainerEq(e4->fi_));
    EXPECT_THAT(fj4, ContainerEq(e4->fj_));
    // check fi_, fj_
    fi5.fill(0), fj5.fill(0);
    fi5[0] = fx5 * e5->ex_(0);
    fi5[1] = fx5 * e5->ex_(1);
    fj5[0] = -fx5 * e5->ex_(0);
    fj5[1] = -fx5 * e5->ex_(1);
    EXPECT_THAT(fi5, ContainerEq(e5->fi_));
    EXPECT_THAT(fj5, ContainerEq(e5->fj_));
    StdArray6d f7;
    for (int i = 0; i < 6; i++)
    {
        f7[i] = fi4[i] + fi5[i];
    }
    // check particle force
    EXPECT_THAT(f7, ContainerEq(p7->force_));
    EXPECT_THAT(fj4, ContainerEq(p8->force_));
    EXPECT_THAT(fj5, ContainerEq(p9->force_));
};


// Tests that the Link3DLD::calcCriticalTimeStep() method.
TEST_F(Link3DLDTest, calcCriticalTimeStep)
{
    cout << "\n---------- Link3DLDTest calcCriticalTimeStep ----------" << endl;
    double dt = e1->length_ / e1->material_->calcCe();
    EXPECT_DOUBLE_EQ(dt, e1->calcCriticalTimeStep());
};


// Tests that the Link3DLD.
inline void oneLink3DLDSystem(Link3DLD* e1, Particle* p1, Particle* p2, double h,
    double zeta, const StdArray6d& force, const DOF &d1, const DOF &d2)
{
    /*
    *                         ^ fy
    *                         |
    *   1                     | 2
    *   .---------------------- -----> fx
    *   /\                    |
    *
    */
    // DOF d1 = {.key={true, true, true, false, false, false},
    //           .val={0, 0, 0, 0, 0, 0}};
    // DOF d2 = {.key={false, true, true, false, false, false},
    //           .val={0, 0, 0, 0, 0, 0}};


    cout << "critical time step: " << e1->calcCriticalTimeStep() << endl;

    int nStep = ceil(50 / h);
    for (int i = 0; i < nStep; i++)
    {
        if (i == 0)
        {
            p2->setForce(force);
            p2->solveFirstStep(h, zeta);
            e1->setParticleForce();
        }
        else
        {
            p2->solve(h, zeta);
            p1->clearForce();
            p2->clearForce();
            e1->setParticleForce();
            p2->addForce(force);
        }
        p1->constraintDof(d1);
        p2->constraintDof(d2);
    }
    cout << "p" << p1->id() << "->force: ";
    printStdArray6d(p1->force_);
    cout << "p" << p2->id() << "->force: ";
    printStdArray6d(p2->force_);
    cout << "e" << e1->id() << "->strain: " << e1->strain_.sx << ", e1->stress: " << endl;
    cout << e1->stress_.sx << ", e1->fx: " << e1->element_force_.fx << endl;
}

// Tests that the Link3DLD.
inline void twoLink3DLDSystem(Link3DLD* e1, Link3DLD* e2, Particle* p1, Particle* p2,
    Particle* p3, double h, double zeta, const StdArray6d& force)
{
    /*
    *                     __ p3
    *                     /
    *                    /
    *                   /
    *                  /
    *                 / e2
    *            fy  /
    *            ^  /
    *            | /
    *            |/
    *  fx -----> /--------------|
    *            p1      e1      p2
    *
    */
    DOF d2 = {.key={true, true, false, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};
    DOF d3 = {.key={true, true, false, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};


    cout << "e1 critical time step: " << e1->calcCriticalTimeStep() << endl;
    cout << "e2 critical time step: " << e2->calcCriticalTimeStep() << endl;

    int nStep = ceil(50 / h);
    for (int i = 0; i < nStep; i++)
    {
        if (i == 0)
        {
            p1->setForce(force);
            p1->solveFirstStep(h, zeta);
            e1->setParticleForce();
            e2->setParticleForce();
        }
        else
        {
            p1->solve(h, zeta);
            p1->clearForce();
            p2->clearForce();
            p3->clearForce();
            e1->setParticleForce();
            e2->setParticleForce();
            p1->addForce(force);
        }
        p2->constraintDof(d2);
        p3->constraintDof(d3);
    }
    cout << "p" << p1->id() << "->force: ";
    printStdArray6d(p1->force_);
    cout << "p" << p2->id() << "->force: ";
    printStdArray6d(p2->force_);
    cout << "p" << p3->id() << "->force: ";
    printStdArray6d(p3->force_);
    cout << "e" << e1->id() << ": ";
    cout << "strain: " << e1->strain_.sx << ", stress: ";
    cout << e1->stress_.sx << ", fx: " << e1->element_force_.fx << endl;
    cout << "e" << e2->id() << ": ";
    cout << "strain: " << e2->strain_.sx << ", stress: ";
    cout << e2->stress_.sx << ", fx: " << e2->element_force_.fx << endl;
}

TEST_F(Link3DLDTest, Link3DLDSystem)
{
    cout << "\n---------- Link3DLDTest Link3DLDSystem ----------" << endl;
    double h = 0.01, zeta = 0.5;
    double strain = 0, stress = 0, fx = 0;
    StdArray6d fi, fj;
    fi.fill(0), fj.fill(0);
    StdArray6d force = {0, 0, 0, 0, 0, 0};

    cout << "\ncheck e1, with positive fx" << endl;
    force[0] = 100;
    DOF d1 = {.key={true, true, true, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};
    DOF d2 = {.key={false, true, true, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};
    oneLink3DLDSystem(e1, p1, p2, h, zeta, force, d1, d2);
    fx = force[0];
    stress = fx / e1->section_->A();
    strain = stress / e1->material_->E();
    fi.fill(0), fj.fill(0);
    fi[0] = fx;
    fj[0] = -fx + force[0];
    // error < 0.5%
    EXPECT_TRUE(fabs(e1->strain_.sx - strain) < 0.005 * fabs(strain));
    EXPECT_TRUE(fabs(e1->stress_.sx - stress) < 0.005 * fabs(stress));
    EXPECT_TRUE(fabs(e1->element_force_.fx - fx) < 0.005 * fabs(fx));
    for (int i = 0; i < 6; i++)
    {
        EXPECT_TRUE(fabs(p1->force_[i] - fi[i]) < 0.001);
        EXPECT_TRUE(fabs(p2->force_[i] - fj[i]) < 0.001);
    }

    cout << "\ncheck e2, with negative fx" << endl;
    force.fill(0), force[2] = -100;
    DOF d3 = {.key={true, true, true, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};
    DOF d4 = {.key={true, true, false, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};
    oneLink3DLDSystem(e2, p3, p4, h, zeta, force, d3, d4);
    fx = force[2];
    stress = fx / e2->section_->A();
    strain = stress / e2->material_->E();
    fi.fill(0), fj.fill(0);
    fi[2] = fx;
    fj[2] = -fx + force[2];
    // error < 0.5%
    EXPECT_TRUE(fabs(e2->strain_.sx - strain) < 0.005 * fabs(strain));
    EXPECT_TRUE(fabs(e2->stress_.sx - stress) < 0.005 * fabs(stress));
    EXPECT_TRUE(fabs(e2->element_force_.fx - fx) < 0.005 * fabs(fx));
    for (int i = 0; i < 6; i++)
    {
        EXPECT_TRUE(fabs(p3->force_[i] - fi[i]) < 0.001);
        EXPECT_TRUE(fabs(p4->force_[i] - fj[i]) < 0.001);
    }

    cout << "\ncheck e3, with fx and fy" << endl;
    force.fill(0), force[0] = -100, force[1] = 20;
    DOF d5 = {.key={true, true, true, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};
    DOF d6 = {.key={false, true, true, false, false, false},
              .val={0, 0, 0, 0, 0, 0}};
    oneLink3DLDSystem(e3, p5, p6, h, zeta, force, d5, d6);
    fx = force[0];
    stress = fx / e3->section_->A();
    strain = stress / e3->material_->E();
    fi.fill(0), fj.fill(0);
    fi[0] = fx;
    fj[0] = -fx + force[0];
    fj[1] = force[1];
    // error < 0.5%
    EXPECT_TRUE(fabs(e3->strain_.sx - strain) < 0.005 * fabs(strain));
    EXPECT_TRUE(fabs(e3->stress_.sx - stress) < 0.005 * fabs(stress));
    EXPECT_TRUE(fabs(e3->element_force_.fx - fx) < 0.005 * fabs(fx));
    for (int i = 0; i < 6; i++)
    {
        EXPECT_TRUE(fabs(p5->force_[i] - fi[i]) < 0.001);
        EXPECT_TRUE(fabs(p6->force_[i] - fj[i]) < 0.001);
    }

    cout << "\ncheck e4 and e5, with fx and fy" << endl;
    force.fill(0), force[0] = -100, force[1] = 20;
    twoLink3DLDSystem(e4, e5, p7, p8, p9, h, zeta, force);

    double strain4 = 0, stress4 = 0, fx4 = 0;
    double strain5 = 0, stress5 = 0, fx5 = 0;
    StdArray6d f7, f8, f9;
    f7.fill(0), f8.fill(0), f9.fill(0);

    fx5 = -force[1] * sqrt(2);
    fx4 = -force[0] - fx5 * sqrt(2) / 2.0;
    stress5 = fx5 / e5->section_->A();
    stress4 = fx4 / e4->section_->A();
    strain5 = stress5 / e5->material_->E();
    strain4 = stress4 / e4->material_->E();

    f7[0] = force[0] + fx4 + fx5 * sqrt(2) / 2.0;
    f7[1] = force[1] + fx5 * sqrt(2) / 2.0;
    f8[0] = -fx4;
    f9[0] = -fx5 * sqrt(2) / 2.0;
    f9[1] = -fx5 * sqrt(2) / 2.0;

    // error < 0.5%
    EXPECT_TRUE(fabs(e4->strain_.sx - strain4) < 0.005 * fabs(strain4));
    EXPECT_TRUE(fabs(e4->stress_.sx - stress4) < 0.005 * fabs(stress4));
    EXPECT_TRUE(fabs(e4->element_force_.fx - fx4) < 0.005 * fabs(fx4));
    EXPECT_TRUE(fabs(e5->strain_.sx - strain5) < 0.005 * fabs(strain5));
    EXPECT_TRUE(fabs(e5->stress_.sx - stress5) < 0.005 * fabs(stress5));
    EXPECT_TRUE(fabs(e5->element_force_.fx - fx5) < 0.005 * fabs(fx5));

    for (int i = 0; i < 6; i++)
    {
        EXPECT_TRUE(fabs(p7->force_[i] - f7[i]) < 0.05);
        EXPECT_TRUE(fabs(p8->force_[i] - f8[i]) < 0.05);
        EXPECT_TRUE(fabs(p9->force_[i] - f9[i]) < 0.05);
    }
};
